/** ###################################################################
**     Filename  : Motologger_v2.C
**     Project   : Motologger_v2
**     Processor : MC9S12XD256BMFU
**     Version   : Driver 01.12
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 11/14/2009, 6:08 PM
**     Abstract  :
**         Main module.
**         Here is to be placed user's code.
**     Settings  :
**     Contents  :
**         No public methods
**
**     (c) Copyright UNIS, spol. s r.o. 1997-2007
**     UNIS, spol. s r.o.
**     Jundrovska 33
**     624 00 Brno
**     Czech Republic
**     http      : www.processorexpert.com
**     mail      : info@processorexpert.com
** ###################################################################*/
/* MODULE Motologger_v2 */

/* Including used modules for compiling procedure */
#include "Cpu.h"
#include "Events.h"
#include "SpeedTimer.h"
#include "TachTimer.h"
#include "LSD0.h"
#include "TimestampTimer.h"
#include "Sample120hz.h"
#include "Keepalive.h"
#include "GPS_SCI.h"
#include "Wifi_SCI.h"
#include "FcnSw.h"
#include "GPS_1PPS.h"
#include "DbgOut.h"
#include "Leds.h"
#include "INT1.h"
#include "SPI1_ACCEL.h"
#include "INT2.h"
#include "ACCEL_CS.h"
#include "Vbat_Gear_ADC.h"
#include "TachOut_PPG.h"
#include "EInt1.h"
#include "LSD1.h"
#include "ScheduleTimer.h"
/* Include shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"

//manual includes
#include "TachOut.h"
#include "stdio.h"
#include "Accel.h"
#include "MMA7455.h"
#include "Scheduler.h"
#include "gps.h"
#include "Fonera.h"
#include "data.h"
#include "Config.h"
#include "SchedulerEvents.h"
#include "TachOut.h"

unsigned short testnum=0;				
unsigned short testval=0;
unsigned short testinput=0;
unsigned char tachsweep=0;
unsigned char accelflag=0;


#define FcnSw_On PTT_PTT6

void doTestToTach(){
	vector_t *pVec;

  for(;;){
  
    if(!FcnSw_On)
      Tach_Out_PutVal(testnum*1000);
   
    switch(testnum){
      case 0:
        DbgOut_PutVal((byte)((1<<testval%8)));
        break;
      case 1:
        Leds_PutVal((byte)testval);
        break;
      case 2:
        Lsds_PutVal((byte)testval);
        break;
      case 3:
        if(FcnSw_On){
          testinput=Gear_GetVal();
          Tach_Out_PutVal(testinput);
        }
        break;
      case 4:
        if(FcnSw_On) {
          testinput=Vbat_GetVal();
          Tach_Out_PutVal(testinput);
        }
        break;
      case 5:
        if(FcnSw_On){
          testinput=TachIn_GetVal();
          Tach_Out_PutVal(testinput);
        }
        break;
      case 6:
        if(FcnSw_On){
          testinput=SpdIn_GetVal();
          Tach_Out_PutVal(testinput);
        }
        break;
      case 7:
        Accel_GetValue(pVec);
        if(FcnSw_On)
          Tach_Out_PutVal((pVec->X+128)*TACH_HI_PEG/255); 
        break;
      case 8:
        Accel_GetValue(pVec);
        if(FcnSw_On)
          Tach_Out_PutVal((pVec->Y+128)*TACH_HI_PEG/255);
        break;
      case 9:
        Accel_GetValue(pVec);
        if(FcnSw_On)
          Tach_Out_PutVal((pVec->Z+128)*TACH_HI_PEG/255);
        break;
      case 10:
        tachsweep=1;
        break;
        
      default:
        Keepalive_PutVal(0); //will reset on next ign off
 
              
    }
   
    //WDog1_Clear();
  }
  
}


void doTestToSerial(){
  vector_t *pVec;
  
  
  
  for(;;){
  	if(accelflag){
  		accelflag=0;
  		Accel_Update();
	    Accel_GetValue(pVec);   
	    printf("X %d Y%d Z%d\r\n",pVec->X,pVec->Y,pVec->Z);
  	}
  }
   
    //WDog1_Clear();
  
}
void led0blink(void){
  Leds_NegBit(0);
}
void led1blink(void){
  Leds_NegBit(1);
}

unsigned char blinkid;
void toggleblink(void){
  if(Scheduler_IsEnabled(blinkid)==1){
    Leds_ClrBit(0);
    Scheduler_DisableEvent(blinkid);
  }
  else
    Scheduler_EnableEvent(blinkid);
}

unsigned short BatteryVoltage;
unsigned short GearPosition;

void measureADC(void){
	Vbat_Gear_ADC_MeasureNotWait();
	/* these are older values */
	BatteryVoltage = Vbat_GetVal();
	GearPosition = Gear_GetVal();
}

unsigned short Speed;
unsigned short Rpm;





extern configFileType Config;

extern unsigned char Fonera_RxBuff[RXBUFFSZ];
extern volatile unsigned char Fonera_DataSize;

extern char GPSData[GPS_NUM_TYPES][MAX_GPSSTR_LEN];
gpsDataType gpsdata;



void getSpeedRPM(void){
  unsigned short displayValue;
 
  Speed = SpdIn_GetVal();
  Rpm	= TachIn_GetVal();
  if( /*(Config.settings[userbuttonMode]==toggleTachModes) ||*/ (FcnSw_GetVal()) ){
    
    if(Config.settings[tachPrimarySetting]==0)
      displayValue = Speed*100;
    else if(Config.settings[tachPrimarySetting]==rpm)
       displayValue = Rpm;   
    else if(Config.settings[tachPrimarySetting]==gear)
       displayValue = Gear_GetVal()*1000; 
    else if(Config.settings[tachPrimarySetting]==tilt)
       displayValue = 10000;
  }else {
   if(Config.settings[tachSecondarySetting]==0)
      displayValue = Speed*100;
   else if(Config.settings[tachSecondarySetting]==rpm)
       displayValue = Rpm;   
   else if(Config.settings[tachSecondarySetting]==gear)
       displayValue = Gear_GetVal()*1000; 
   else if(Config.settings[tachSecondarySetting]==tilt)
       displayValue = 10000;
    
  }
  
  TachOut_PutVal(displayValue);
  
  //LSD0_SetFreqHz(x++%318+3);
}

void main(void)
{
  /* Write your local variable definition here */
  char run=0;
  commandDataType cmd;
  commandDataType newFile;
  int i;
  int ret;
  unsigned char foo;
  //while(!run){}//wait

  //DO THIS FIRST: setup default config
  Config_Init();

  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
  PE_low_level_init();
  /*** End of Processor Expert internal initialization.                    ***/
  
  //any magic that mus happen before a sample is taken goes here
  
  //enable all timed events
  Sample120hz_Enable();

  Scheduler_Init();
  SpdIn_Init();
  
  //each tick is 400ns
  //Scheduler_AddEvent_ticks(&led0blink,125000);  //0.05s
  //Scheduler_AddEvent_hz(&led1blink,20);
  
  //set up all sampling events
  SchedulerEvents_Init();

  Scheduler_AddEvent_ticks(&measureADC,125000); //Fs = 20 Hz sample rate
  Scheduler_AddEvent_ticks(&getSpeedRPM,256000); //Fs = 10 Hz sample rate
  
  
  //Scheduler_AddEvent_ticks(&toggleblink,12500000); //5s
  newFile.tag=command_tag; 
  newFile.command=foneraStartNewFile;
  
  //printf("test output\r\n");
  //doTestToSerial();
  for(;;){  	
    if(Fonera_GetFlag()){	
      if(Fonera_Receive(&cmd, sizeof(cmd))){
        if(Fonera_RxBuff[0]=='c'){
        	//Fonera_Send((char*)&cmd,sizeof(cmd));
        	//update config file
        	if(outputSamplesEnabled==cmd.command
        	&&1==cmd.param
        	&&0==Config.settings[outputSamplesEnabled]){
        		SchedulerEvents_LoadConfig();
        	  Fonera_Send((char*)&newFile,sizeof(newFile));
        	}
      	  Config.settings[cmd.command]=cmd.param;
        }
      }
     
    }
    
  	
    if(1==Config.settings[outputSamplesEnabled]){
    	

	    if( GPS_getUpdateFlag() ){
	   		parseGpsData();
	    	foo=GPS_LastParsedType(); //for some reason only works like this?!
	    	if(1 == Config.settings[gpsSampling]){
	    	  if(TYPE_GPGGA==foo){
	   		  //construct and send gps packet
	   		    gpsdata.tag=GPS_tag;
	   		    memcpy(gpsdata.gpsData,GPSData[TYPE_GPGGA],100);
	   		    gpsdata.time_stamp=Timestamp_GetVal();
	   		    Fonera_Send((char*)&gpsdata, sizeof(gpsdata));
	    	  }
	   		}
	    }
    
    
    	SchedulerEvents_RunAllFlagged();
    
    } else if( GPS_getUpdateFlag() ){
	   		parseGpsData();
  	}

        
  }
  
  /*
  for(;;){
  	if( GPS_getUpdateFlag() ){
   		parseGpsData();
   		printf("%s\n",GPSData[TYPE_GPGGA]);
   		printf("%s\n",GPSData[TYPE_GPRMC]);
   		printf("%s\n",GPSData[TYPE_GPGSA]);
   
    //memcpy();
    //	fonera_print
    }
  }*/


  /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
  for(;;){}
  /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/



/* END Motologger_v2 */
/*
** ###################################################################
**
**     This file was created by UNIS Processor Expert 2.98 [03.98]
**     for the Freescale HCS12X series of microcontrollers.
**
** ###################################################################
*/
